;#INCLUDE <P16F877a.inc> ;windows
#INCLUDE <P16F877A.INC> ;linux

#DEFINE BANK0 BCF STATUS,RP0
#DEFINE BANK1 BSF STATUS,RP0

#DEFINE MOTOR_PORT PORTD

#DEFINE TURN_LEFT_VALUE B'10000000'
#DEFINE TURN_RIGHT_VALUE B'00010000'
#DEFINE STRAIGHT_VALUE B'10010000'

;bits of flag
#DEFINE VELOCITY_FLAG_BIT 1    ;1 = FULL 0 = 3/5
#DEFINE PWM_STATE_FLAG_BIT 0   ;0 = go up 1 = go down

;3/5 of full velocity
#DEFINE PWM_HIGH .102
#DEFINE PWM_LOW .153

;only timer interruption enable 
#DEFINE INTCON_SETUP B'10101000'

#DEFINE INFRARED_PORT PORTB

;INICIO
ORG 0x00
   GOTO INICIO

;AREA PARA INTERRUPCOES
ORG 0x04
   GOTO INTERRUPTION

;DECLARAR VARIAVEIS
CBLOCK 0x20
   AUX
   MOTOR_OUTPUT
   FLAG
ENDC

INICIO:
   MOVLW .0
   MOVWF FLAG
   MOVLW PWM_LOW
   MOVWF TMR0

   ;CONFIGURAR PORTAS
   BANK1
   MOVLW B'00000000'
   MOVWF TRISA
   MOVLW B'00001110' ;B1, B2 and B3 connected to infrared sensors
   MOVWF TRISB
   MOVLW B'00000000'
   MOVWF TRISC
   MOVLW B'00000000' ;D4 and D7 connected to motors
   MOVWF TRISD
   MOVLW B'00000000'
   MOVWF TRISE

   MOVLW B'00000000'
   MOVWF OPTION_REG
   MOVLW INTCON_SETUP
   MOVWF INTCON
   BANK0

  ; PROGRAMA PRINCIPAL
MAIN:
    MOVFW INFRARED_PORT
    MOVWF AUX
    RRF AUX               ;roll 1 time because the first port of infrared is 1
    MOVLW B'00000111'
    ANDWF AUX,D
    MOVFW AUX
    ADDWF PCL,D           ; datasheet said to use D instead of F
    GOTO OUT_OF_TAPE ;000
    GOTO TURN_RIGHT ;001
    GOTO STRAIGHT ;010
    GOTO TURN_RIGHT_FULL ;011
    GOTO TURN_LEFT ;100
    GOTO UNDEF ;101
    GOTO TURN_LEFT_FULL ;110
    GOTO UNDEF ;111

UNDEF:
OUT_OF_TAPE:
STRAIGHT:
    MOVLW STRAIGHT_VALUE
    MOVWF MOTOR_OUTPUT
    BSF FLAG,VELOCITY_FLAG_BIT
    GOTO MAIN

TURN_LEFT_FULL:
    MOVLW TURN_LEFT_VALUE
    MOVWF MOTOR_OUTPUT
    BSF FLAG,VELOCITY_FLAG_BIT
    GOTO MAIN

TURN_LEFT:
    MOVLW TURN_LEFT_VALUE
    MOVWF MOTOR_OUTPUT
    BCF FLAG,VELOCITY_FLAG_BIT
    GOTO MAIN

TURN_RIGHT_FULL:
    MOVLW TURN_RIGHT_VALUE
    MOVWF MOTOR_OUTPUT
    BSF FLAG,VELOCITY_FLAG_BIT
    GOTO MAIN

TURN_RIGHT:
    MOVLW TURN_RIGHT_VALUE
    MOVWF MOTOR_OUTPUT
    BCF FLAG,VELOCITY_FLAG_BIT
    GOTO MAIN

INTERRUPTION:
   BTFSS FLAG,VELOCITY_FLAG_BIT
   GOTO NOT_FULL_VELOCITY
   MOVFW MOTOR_OUTPUT
   MOVWF MOTOR_PORT
   MOVLW PWM_LOW      ;shortest time until another interruption
   MOVWF TMR0
   GOTO RECONF_TIMER

NOT_FULL_VELOCITY:
   BTFSS FLAG,PWM_STATE_FLAG_BIT
   GOTO UP_TIME   ; =0
   GOTO DOWN_TIME ; =1

DOWN_TIME:
   MOVLW .0
   MOVWF MOTOR_PORT
   BCF FLAG,PWM_STATE_FLAG_BIT
   MOVLW PWM_LOW
   MOVWF TMR0
   GOTO RECONF_TIMER

UP_TIME:
   MOVFW MOTOR_OUTPUT
   MOVWF MOTOR_PORT
   BSF FLAG,PWM_STATE_FLAG_BIT
   MOVLW PWM_HIGH
   MOVWF TMR0
   GOTO RECONF_TIMER

RECONF_TIMER:
   BANK1
   MOVLW INTCON_SETUP
   MOVWF INTCON
   BANK0
RETFIE

END
